#include <ctime>
#include <ctime>
#include <cmath>
#include <cstdio>

#include "Types.h"
#include "BigVector.h"
#include "DynamicDeformationState.h"
#include "TetrahedronState.h"
#include "Mechanics.h"
#include "FEMModel.h"
#include "Setting.h"
#include "Statistics.h"
#include "Node.h"
#include "Simplex.h"




bool node_viscosity;
bool spring_viscosity;
bool spring_force;


void DynamicDeformationState::ComputeResultantForce (Real * force,
													Real const * velocity,
													Real const * displacement)
{
	int n = Dimension ();

	BigVectorNullify (force, n);
	AddElasticityForce (force, displacement);
	AddViscosityForce (force, displacement, velocity);
	BigVectorAdd (force, external_force_.UnsafeAccessArray (),force, n);

	flop_count += n ;
}

void DynamicDeformationState::ComputeAcceleration (Real * acc,
												 Real const* velocity,
												 Real const* displacement)
{
	int n = Dimension ();
	BigVectorNullify (acc, n);
	ComputeResultantForce (acc, velocity, displacement);
	Real * m = inverse_masses_.UnsafeAccessArray ();
	BigVectorPointwiseMultiply (acc, m, acc, n);
	BigVectorPartialNullify (acc, acc, fixed_arr_.AccessArray (), n);

	flop_count += 2 * n ;
}


/*
TODO: move to deformation-state.cc
*/
void DynamicDeformationState::AddElasticityForce (Real * acc,
												 Real const* displacement)
{
	if (force_function_)
	{
		ApplyForceFunction (elasticity_.UnsafeAccessArray (), displacement);
		BigVectorAdd (acc, elasticity_.AccessArray (), acc, Dimension());
	}
	else if (spring_force)
	{
		TetrahedronState **start = tet_states_.AccessArray ();
		TetrahedronState **end = start + tet_states_.Size ();

		for (TetrahedronState** t = start; t != end; t++)
		{
			if (spring_force)
				flop_count += SimpleSpringForce (*t, acc, displacement, fem_);
		}
	}
}

void DynamicDeformationState::AddViscosityForce (Real *force ,
												Real const *velocity,
												Real const *displacement)
{
	if (spring_viscosity)
	{
		TetrahedronState **start = tet_states_.AccessArray ();
		TetrahedronState **end = start + tet_states_.Size ();

		for (TetrahedronState** t = start; t != end; t++)
			//	flop_count +=
			SimpleSpringViscosityForce (*t, force, displacement, velocity, viscosity_, fem_);
	}
	if (node_viscosity)
	{
		// TODO: lumped viscosity
		Real * m = scratch4_.UnsafeAccessArray ();
		int n = Dimension();
		BigVectorPointwiseMultiply (m, lumped_masses_.AccessArray(), velocity, n);
		BigVectorSaxpy (force, -viscosity_, m, force, n);
	}



	flop_count += Dimension ();
}

/****************************************************************
TIME INTEGRATION
****************************************************************/

/*
Simple euler forward.
*/
void DynamicDeformationState::EulerForward (DynamicDeformationState*me,Real delta_t)
{
	int n = me->Dimension ();

	Real *v = me->velocity_.UnsafeAccessArray ();
	Real *f =me-> scratch1_.UnsafeAccessArray ();
	Real *d = me->displacement_.UnsafeAccessArray ();

	me->ComputeAcceleration (f, v, d);
	BigVectorSaxpy (d, delta_t, v, d, n);
	BigVectorSaxpy (v, delta_t, f, v, n);

	BigVectorPartialNullify (v, v, me->fixed_arr_.AccessArray (), n);
}

void
DynamicDeformationState::ClassicalRungeKutta (DynamicDeformationState*me,Real delta_t)
{
	int n = me->Dimension ();

	Real * a1 = me->scratch0_.UnsafeAccessArray ();
	Real * v = me->velocity_.UnsafeAccessArray ();
	Real * d = me->displacement_.UnsafeAccessArray ();
	Real * v1 =me->scratch1_.UnsafeAccessArray ();
	Real * d1 = me->scratch2_.UnsafeAccessArray ();
	Real * kv = me->scratch3_.UnsafeAccessArray ();
	Real * ka = me->scratch4_.UnsafeAccessArray ();

	BigVectorPartialNullify (v, v, me->fixed_arr_.AccessArray (), n);
	BigVectorCopy (v1, v, n);
	BigVectorCopy (d1, d, n);  
	me->ComputeAcceleration (a1, v1, d1);
	BigVectorCopy (ka, a1, n);
	BigVectorCopy (kv, v1, n);

	BigVectorSaxpy (d1, delta_t* 0.5, v1, d1, n);
	BigVectorSaxpy (v1, delta_t* 0.5, a1, v1, n);  
	me->ComputeAcceleration (a1, v1, d1);
	BigVectorSaxpy (ka, 2.0, a1, ka, n);
	BigVectorSaxpy (kv, 2.0, v1, kv, n);

	BigVectorSaxpy (d1, delta_t* 0.5, v1, d1, n);
	BigVectorSaxpy (v1, delta_t* 0.5, a1, v1, n);  
	me->ComputeAcceleration (a1, v1, d1);
	BigVectorSaxpy (ka, 2.0, a1, ka, n);
	BigVectorSaxpy (kv, 2.0, v1, kv, n);

	BigVectorSaxpy (d1, delta_t, v1, d1, n);
	BigVectorSaxpy (v1, delta_t, a1, v1, n);  
	me->ComputeAcceleration (a1, v1, d1);
	BigVectorAdd (ka, ka, a1, n);
	BigVectorAdd (kv, kv, v1, n);

	BigVectorSaxpy (d, 1.0/6.0 * delta_t, kv, d, n);
	BigVectorSaxpy (v, 1.0/6.0 * delta_t, ka, v, n);  

#ifndef NDEBUG
	Real vlen = BigVectorIP (v, v, n);
	assert (!isinf (vlen) && !isnan (vlen));
#endif

	flop_count += 26 * n;

	global_iteration_count ++;
}  


/*
Explicit scheme from the Picinbono ICRA 2001 paper.

This scheme is a SS22 multistep order 2 with theta_1=1/2 and theta_2 = 0.

(see Zienkewicz vol. 1, 5th ed, page 530)

Conditionally stable for time-step delta_t < h * sqrt(density/stiffness).

We have h = Oh(n^{1/3})


Note that the viscosity in their paper (gamma_i) is negative, while
ours is positive.

COST: 14 * n
*/
void DynamicDeformationState::ExplicitSS22 (DynamicDeformationState*me,Real delta_t)
{
	int n = me->Dimension ();
	Real * inv_factor = me->scratch0_.UnsafeAccessArray ();
	Real * this_t_contribution = me->scratch1_.UnsafeAccessArray ();
	Real * prev_t_contribution = me->scratch2_.UnsafeAccessArray ();
	Real * force_contribution = me->scratch3_.UnsafeAccessArray ();
	Real * prev_t_displacements = me->velocity_.UnsafeAccessArray ();
	Real * d = me->displacement_.UnsafeAccessArray ();
	Real * m = me->lumped_masses_.UnsafeAccessArray ();

	BigVectorPointwiseMultiply (prev_t_contribution, m, prev_t_displacements, n);

	BigVectorScale (prev_t_contribution, (2 - me->viscosity_ * delta_t)/(2*Sqr (delta_t)), prev_t_contribution, n);

	BigVectorPointwiseMultiply (this_t_contribution, m, d, n);
	BigVectorScale (this_t_contribution, 2.0 / Sqr(delta_t), this_t_contribution, n);

	BigVectorNullify (force_contribution, n);
	me->AddElasticityForce (force_contribution, d);
	BigVectorAdd (force_contribution, force_contribution, me->external_force_.UnsafeAccessArray (), n);

	BigVectorAdd (this_t_contribution, this_t_contribution, force_contribution,n );

	BigVectorSubtract (this_t_contribution, this_t_contribution,prev_t_contribution, n);

	BigVectorScale (inv_factor, (2.0 + me->viscosity_ * delta_t) /(2*Sqr(delta_t)), m, n );
	BigVectorPointwiseInverse (inv_factor, inv_factor, n);

	BigVectorPointwiseMultiply (this_t_contribution, this_t_contribution, inv_factor,n);

	BigVectorCopy (prev_t_displacements, d,n);
	BigVectorPartialCopy (d, me->fixed_arr_.AccessArray (), this_t_contribution,n);
	BigVectorPartialNullify (force_contribution,  force_contribution, me->fixed_arr_.AccessArray (), n);

	flop_count += 14 * n ;
}



/****************************************************************/


LinkArray< Array<Real> > DynamicDeformationState::VectorVariables ()
{
	Array<Real> *vecs[] = {
		&lumped_masses_, &inverse_masses_,
		&scratch0_, &scratch1_, &scratch2_, &scratch3_, &scratch4_,
		&elasticity_, &velocity_,0 };

		LinkArray< Array<Real> > vvs (DeformationState::VectorVariables ());
		for (int i =0; vecs[i]; i++)
			vvs.Push (vecs[i]);

		return vvs;
}


/*
We don't use CPU time for the time-integration, because it is not
constant, certainly not on our platform.
*/

void DynamicDeformationState::DoSimulationBody ()
{
	Real  now = clock () / Real (CLOCKS_PER_SEC);
	Real  elapsed = now - last_time_;
	last_time_ = now;

	/*
	let's not get into time-scale difficulties.
	*/
	elapsed = time_scale_;

	(*TimeIntegrationFunction_)(this, elapsed);

	if (!(global_iteration_count % 5))
		StatisticTick ();
}

void DynamicDeformationState::InitSimulation ()
{
	DeformationState::InitSimulation ();
	last_time_ = clock () / Real (CLOCKS_PER_SEC);
}

DynamicDeformationState::DynamicDeformationState (FiniteElementModel*fem)
		: DeformationState (fem)
{
	viscosity_ = GetNumberSetting ("viscosity");
	density_ = GetNumberSetting ("density");

	char const * vis = GetStringSetting ("viscosity");
	spring_viscosity = !strcmp(vis, "spring");
	node_viscosity = !strcmp(vis, "node");

	char const * elas = GetStringSetting ("elasticity");
	spring_force = !strcmp(elas, "springs");

	char const * integr = GetStringSetting ("time-integration");
	if( !strcmp(integr, "RK4"))
		TimeIntegrationFunction_ = &ClassicalRungeKutta;
	else if (!strcmp(integr, "eulerfw"))
		TimeIntegrationFunction_ = &EulerForward;
	else if (!strcmp (integr, "explicit-ss22"))
		TimeIntegrationFunction_ = &ExplicitSS22;
	printf (" using: viscosity = %s, elasticity=  %s,\ntime-integration = %s\n",
		vis, elas, integr);


	InitLameParameters();
	Real critter =CriticalTimeStep ();
	time_scale_ = GetNumberSetting ("time-scale");
	printf ("relative to dt_crit = %f\n", time_scale_/critter);

#if 0
	/*
	Fix this up:
	*/

	Real crit_damping  = sqrt ( GetNumberSetting ("young") * GetNumberSetting ("density"));
	printf ("Damping relative to critical = %f\n", GetNumberSetting ("viscosity") / crit_damping);
#endif
}



Real
DynamicDeformationState::WaveSpeed ()const
{
	Real young = GetNumberSetting ("young");

	return sqrt (young / GetNumberSetting ("density"));
}

Real DynamicDeformationState::CriticalTimeStep ()const
{
	if (TimeIntegrationFunction_ == &ExplicitSS22)
		return ExplicitSS22CriticalTimeStep ();
	else if (TimeIntegrationFunction_ == &EulerForward)
		return EulerForwardCriticalTimeStep ();
	else if (TimeIntegrationFunction_ == &ClassicalRungeKutta)
		/*
		I think the stability is equal to euler FW stability for decay
		equations.  */
		return 1.0;

	return 0.0;
}

Real DynamicDeformationState::ExplicitSS22CriticalTimeStep () const
{
	/*
	assuming lumped masses (see Zienkewicz vol I p 520).

	return sqrt (2) * minimum_element_length (fem_) / wave_speed ();
	*/


	/*
	Apparently, this is called the Courant condition. 0.7 is a figment of the imagination.
	*/
	return 0.7* MinimumElementLength (fem_) *  sqrt (GetNumberSetting ("density")/ (lame_lambda + 2 * lame_mu));  
}

Real DynamicDeformationState::EulerForwardCriticalTimeStep ()const
{
	/*
	Zienkewicz p 498, 501, 502

	2D Euler FW is a single step Taylor serie collocation  of Euler FW 1D.

	Critical time step is Oh(h^2), which isn't all that good for FE computations.


	(? weird: lame parameters are no part of this?? )
	*/

	return Sqr (MinimumElementLength (fem_)) * viscosity_ / density_ ;
}

void
DynamicDeformationState::DuplicateVariable (int dst, int src )
{
	DeformationState::DuplicateVariable (dst,src);
	velocity_[dst] = velocity_[src];
	lumped_masses_[dst] = lumped_masses_[src]; // guh.!
}

void
DynamicDeformationState::UpdateVectorSizes ()
{
	DeformationState::UpdateVectorSizes ();
	if (TimeIntegrationFunction_ == &ExplicitSS22)
		velocity_ = displacement_;

	int  n = fem_->NodeCount () * 3;

	BigVectorNullify (lumped_masses_.UnsafeAccessArray (),n);

	Array<Real> lv (LumpedVolumes (fem_));

	//  total_mass / fem_->node_count ();
	for (int i = lv.Size () ; i--;)
		for (int j = 3;j--;)
		{
			lumped_masses_[3*i + j] =  lv[i] * density_; 
		}

		BigVectorPointwiseInverse (inverse_masses_.UnsafeAccessArray (),
			lumped_masses_.AccessArray (),
			Dimension());

}



